#!/usr/bin/env python3

import sys
import signal
import time
from i2cdev import I2C
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

# 全局速度系数 (0.5 = 50%速度)
GLOBAL_SPEED_SCALE = 0.5

# I2C配置
DEFAULT_I2C_BUS = 5
DEFAULT_I2C_ADDRESS = 0x24

# ADC配置
ADC_MAX_VALUE = 1023  # 10位ADC，范围0-1023

# 定义寄存器地址
ADDRESS_VERSION = 0x00
ADDRESS_IO_MODE = 0x01
ADDRESS_ANALOG_VALUES = 0x10
ADDRESS_VOLTAGE_VALUES = 0x20
ADDRESS_RATIO_VOLTAGE = 0x30
ADDRESS_DIGITAL_VALUES = 0x40
ADDRESS_PWM_DUTY = 0x50
ADDRESS_PWM_FREQUENCY = 0x60

class GpioExpansionBoard:
    def __init__(self, device_i2c_address=DEFAULT_I2C_ADDRESS, bus=DEFAULT_I2C_BUS):
        self.device_i2c_address = device_i2c_address
        self.i2c = I2C(device_i2c_address, bus)

    def get_gpio_adc_value(self, gpio_pin):
        try:
            # 写入要读取的寄存器地址
            register = ADDRESS_ANALOG_VALUES + gpio_pin * 2  # sizeof(uint16_t) = 2
            # 将寄存器地址转换为bytes
            self.i2c.write(bytes([register]))
            
            # 读取2个字节的ADC值
            data = self.i2c.read(2)
            
            # 转换为16位整数（小端序，与Arduino代码保持一致）
            analog_value = int.from_bytes(data, byteorder='little')
            return analog_value
            
        except Exception as e:
            print(f"读取ADC时发生错误: {e}")
            return 0

class JoystickTeleopNode(Node):
    def __init__(self, board):
        super().__init__('joystick_teleop')
        self.board = board
        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)
        
        # 速度参数设置 (应用全局速度系数)
        self.max_linear_speed = 0.5 * GLOBAL_SPEED_SCALE  # 最大线速度 (m/s)
        self.max_angular_speed = 1.0 * GLOBAL_SPEED_SCALE  # 最大角速度 (rad/s)
        
    def timer_callback(self):
        # 读取摇杆值
        joystick_x = self.board.get_gpio_adc_value(0)
        joystick_y = self.board.get_gpio_adc_value(1)
        
        # 将ADC值转换为-1到1的范围
        x_norm = (joystick_x - ADC_MAX_VALUE/2) / (ADC_MAX_VALUE/2)
        y_norm = (joystick_y - ADC_MAX_VALUE/2) / (ADC_MAX_VALUE/2)
        
        # 创建Twist消息
        msg = Twist()
        # Y轴控制线速度 (保持负号)
        msg.linear.x = y_norm * self.max_linear_speed
        # X轴控制角速度 (移除负号来翻转方向)
        msg.angular.z = x_norm * self.max_angular_speed
        
        # 发布消息
        self.publisher.publish(msg)
        
        # 显示当前速度值
        print(f"线速度: {msg.linear.x:.2f} m/s, 角速度: {msg.angular.z:.2f} rad/s", end='\r')

def main():
    # 初始化ROS2
    rclpy.init()
    
    # 创建GPIO扩展板对象
    board = GpioExpansionBoard()
    
    # 创建并运行节点
    node = JoystickTeleopNode(board)
    print("摇杆遥控已启动... 按 CTRL+C 退出")
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()